#include "Line.h"

void Line::clean_line()
{
	//may need to deop one or two
	bool collide = false;

	int l_right_point = 0;				// the right bound of the source point

	int l_left_point = 0;				// the left bound of the source point
	
	int curr_p , next_p;

	int mid_p = l_c[0]; //x the main coordinate of the source
	int vertical  = 0;
	if(l_c[2] == 0.25 || l_c[2] == 0.75)
	{//y
		vertical = 1;
		mid_p = l_c[1];
	}
	for (int i = 0; i < l_points.size()-1;i++)
	{// can be change to biary search
		curr_p = l_points[i][vertical];
		next_p = l_points[i+1][vertical];

		if(curr_p<mid_p && next_p>mid_p)
		{
			l_right_point = next_p;
			l_left_point = curr_p;
			break;
		}
		else if(i==0 && mid_p<curr_p)
		{
			l_right_point = curr_p;
			l_left_point = curr_p;
			break;
		}
		else if(i == l_points.size()-2 && mid_p > next_p)
		{
			l_right_point = next_p;
			l_left_point = next_p;
			break;
		}
	}

	for (int i = l_right_point; i < l_points.size(); i++)
	{
		if(collide || !(l_collision->valid(l_points[i])))
		{
			collide = true;
			l_points.erase(l_points.begin()+i);
		}
	}
	collide=false;
	for (int i = l_left_point; i >= 0; i--)
	{
		if(collide || !(l_collision->valid(l_points[i])))
		{
			collide = true;
			l_points.erase(l_points.begin()+i);
		}
	}
}

void Line::change_line(Point p)
{
	l_c=p;
	build_line();
	clean_line();
}

Point Line::get_rand_point()
{
	int index = rand() % l_points.size();
	return l_points[index];
}

void Line::build_line()
{
	//need to change how the robot move TODOs
	double left_p = -1;			//the left limit of workspace
	double curr_p = left_p+l_rate;
	double right_p = 1;			//the right limit of workspace
	bool skip_source=true;		// if the source point is part of the line we skip it
	
	int mid_p = l_c[0]; //x the main coordinate of the source
	if(l_c[2] == 0.25 || l_c[2] == 0.75)
	{//y
		mid_p = l_c[1];
	}

	while (curr_p < right_p)
	{
		if(skip_source && curr_p == mid_p)
		{//if the current point is a sample point
			skip_source = false;
			curr_p+=l_rate;

			if(curr_p + l_rate >= right_p)
			{//and the next point is outside the workspace
				break;
			}
		}
		
		//chekc what to do whith l_c
		l_points.push_back(line_eq(curr_p));
		curr_p+=l_rate;
	}
}

Point Line::line_eq(double x)
{
	
	Point p;

	if(l_c[2] == 0.25 || l_c[2] == 0.75)
	{// if the line is vertical
		p = Point(l_c[0], x , l_c[2]); 
	}
	else
	{// y-y0 = m(x-x0)
		p = Point(x , tan(l_c[2]*360)*(x-l_c[0])+l_c[1] , l_c[2]);
	}

	return p;
}
